#include "bits/stdc++.h"
#include "path.h"
#include "unistd.h"

using namespace std;

#include "ros/ros.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <sensor_msgs/LaserScan.h>

auto pt        = new path();
auto msg_copy  = new my_msg::LaserScans();
int  generated = 0;

void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    msg_copy->angle_min       = msg->angle_min;
    msg_copy->angle_increment = msg->angle_increment;
    size_t t                  = msg->ranges.size();
    for ( size_t i = 0; i < t; i++ ) {
        msg_copy->ranges[i] = msg->ranges[i];
    }

    int t1 = pt->get_position(msg_copy);
    if ( t1 == 2 ) {
        ros::param::set("stim", 1);
        // 写死
        for ( int i = 0; i < 75; i++ ) {
            ros::param::set("gear", 100);
            ros::param::set("speed", 3);
            usleep(10000);
        }
        for ( int i = 0; i < 75; i++ ) {
            ros::param::set("gear", -100);
            ros::param::set("speed", 3);
            usleep(10000);
        }
    }
    else if ( t1 == 3 ) {
        double min = 100;
        for ( i = 224; i < 227; i++ ) min = msg_copy->ranges[i] < min ? msg_copy->ranges[i] : min;
        if ( min < 0.8 ) {
            ros::param::set("state", END);
            ros::param::set("gear", 0);
            ros::param::set("speed", 0);
            return;
        }
        if ( min > 1.5 ) {
            ros::param::set("gear", -100);
            ros::param::set("speed", 3);
            return;
        }
        int sum_left = 0, sum_right = 0;
        for ( int i = 220; i < 225; i++ ) sum_left += msg_copy->ranges[i];
        for ( int i = 226; i < 231; i++ ) sum_left += msg_copy->ranges[i];
        if ( (sum_left - sum_right) > 0.2 ) {
            ros::param::set("gear", -50);
            ros::param::set("speed", 3);
        }
        else if ( (sum_left - sum_right) < -0.2 ) {
            ros::param::set("gear", 50);
            ros::param::set("speed", 3);
        }
        else {
            ros::param::set("gear", 0);
            ros::param::set("speed", 3);
        }
    }
    else {
        return;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "path");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 1, laserCallback);
    ros::Rate       looprate(50);

    int state = 0;
    int type  = 0;
    pt->type  = SIDE;
    int stim  = 0;
    while ( ros::ok() ) {
        ros::param::get("state", state);
        ros::param::get("stim", stim);
        if ( state == DAOCHE ) {
            ros::spinOnce();
        }
        looprate.sleep();
    }
    return 0;
}
